package kit.edu.lego.gruppe3.levels.fallingBridge;

import kit.edu.lego.gruppe3.ComponentsRepository;
import kit.edu.lego.gruppe3.levels.LevelSelector;
import lejos.nxt.LCD;
import lejos.robotics.navigation.DifferentialPilot;
import lejos.robotics.subsumption.Behavior;

/**
 * Rotates the robot a little to the left when he hits the wall on his right
 * @author Lac
 *
 */
public class RightWallEvader implements Behavior
{

	private DifferentialPilot robot = ComponentsRepository.getInstance().getRobotPilot();

	@Override
	public void action()
	{
		LCD.drawString("B: Wall", 0, 1);

		robot.setRotateSpeed(90);
		robot.setTravelSpeed(5);
		robot.rotate(10);
	}

	@Override
	public void suppress()
	{

	}

	@Override
	public boolean takeControl() 
	{
		LCD.drawString("Touch: "+ComponentsRepository.getInstance().getLeftTouchSensor().isPressed(), 0,5);
		LCD.drawString("Sonic: "+ComponentsRepository.getInstance().getSonicSensor().getDistance(), 0, 6);
		LCD.refresh();
		return LevelSelector.getInstance().isFallingBridge() &&
			ComponentsRepository.getInstance().getSonicSensor().getDistance() <= 10;

	}

}
